#include "utils.h"
#include "control/control_truck.h"
#include "spdlog/spdlog.h"
#include "spdlog/sinks/stdout_color_sinks.h"
#include "spdlog/sinks/rotating_file_sink.h"

namespace control
{


Pose_type getTruckProjectPoint2Line(Pose_type &p1, Pose_type &p2, geometry_msgs::msg::Pose2D &p0)
{
    double vec_1_x       = p0.x - p1.x;
    double vec_1_y       = p0.y - p1.y;
    double vec_2_x       = p2.x - p1.x;
    double vec_2_y       = p2.y - p1.y;
    double vec1_dot_vec2 = vec_1_x * vec_2_x + vec_1_y * vec_2_y;
    double dis_start_end = vec_2_x * vec_2_x + vec_2_y * vec_2_y;
    double k_scaler      = vec1_dot_vec2/dis_start_end;
    double projection_x  = k_scaler * vec_2_x + p1.x;
    double projection_y  = k_scaler * vec_2_y + p1.y;

    Pose_type projectionPoint;
    projectionPoint.x    = projection_x;
    projectionPoint.y    = projection_y;

    return projectionPoint;
}


ControlTruck::ControlTruck() 
{
	RCLCPP_INFO(node->get_logger(),"Start to load parameters.");
	// ros::NodeHandle private_nh_("~");
	SpdlogInit();
	#if 1
	node->declare_parameter<int>("cycle_frequency",cycle_frequency_);
	cycle_frequency_ = node->get_parameter("cycle_frequency").as_int();
	node->declare_parameter<double>("adaptive_kp_line",controlParamInfo_.adaptive_kp_line);
	controlParamInfo_.adaptive_kp_line = node->get_parameter("adaptive_kp_line").as_double();
	node->declare_parameter<double>("adaptive_ki_line",controlParamInfo_.adaptive_ki_line);
	controlParamInfo_.adaptive_ki_line = node->get_parameter("adaptive_ki_line").as_double();
	node->declare_parameter<double>("adaptive_kd_line",controlParamInfo_.adaptive_kd_line);
	controlParamInfo_.adaptive_kd_line = node->get_parameter("adaptive_kd_line").as_double();
	node->declare_parameter<double>("adaptive_kp_curve",controlParamInfo_.adaptive_kp_curve);
	controlParamInfo_.adaptive_kp_curve = node->get_parameter("adaptive_kp_curve").as_double();
	node->declare_parameter<double>("adaptive_ki_curve",controlParamInfo_.adaptive_ki_curve);
	controlParamInfo_.adaptive_ki_curve = node->get_parameter("adaptive_ki_curve").as_double();
	node->declare_parameter<double>("adaptive_kd_curve",controlParamInfo_.adaptive_kd_curve);
	controlParamInfo_.adaptive_kd_curve = node->get_parameter("adaptive_kd_curve").as_double();
	node->declare_parameter<double>("adaptive_kp_heading_line",controlParamInfo_.adaptive_kp_heading_line);
	controlParamInfo_.adaptive_kp_heading_line = node->get_parameter("adaptive_kp_heading_line").as_double();
	node->declare_parameter<double>("adaptive_ki_heading_line",controlParamInfo_.adaptive_ki_heading_line);
	controlParamInfo_.adaptive_ki_heading_line = node->get_parameter("adaptive_ki_heading_line").as_double();
	node->declare_parameter<double>("adaptive_kd_heading_line",controlParamInfo_.adaptive_kd_heading_line);
	controlParamInfo_.adaptive_kd_heading_line = node->get_parameter("adaptive_kd_heading_line").as_double();
	node->declare_parameter<double>("adaptive_kp_heading_curve",controlParamInfo_.adaptive_kp_heading_curve);
	controlParamInfo_.adaptive_kp_heading_curve = node->get_parameter("adaptive_kp_heading_curve").as_double();
	node->declare_parameter<double>("adaptive_ki_heading_curve",controlParamInfo_.adaptive_ki_heading_curve);
	controlParamInfo_.adaptive_ki_heading_curve = node->get_parameter("adaptive_ki_heading_curve").as_double();
	node->declare_parameter<double>("adaptive_kd_heading_curve",controlParamInfo_.adaptive_kd_heading_curve);
	controlParamInfo_.adaptive_kd_heading_curve = node->get_parameter("adaptive_kd_heading_curve").as_double();
	node->declare_parameter<double>("position_jump",position_jump_);
	position_jump_ = node->get_parameter("position_jump").as_double();
	node->declare_parameter<double>("heading_jump",heading_jump_);
	heading_jump_ = node->get_parameter("heading_jump").as_double();
	node->declare_parameter<double>("replan_angle_min",replan_angle_min_);
	replan_angle_min_ = node->get_parameter("replan_angle_min").as_double();
	node->declare_parameter<double>("replan_angle_max",replan_angle_max_);
	replan_angle_max_ = node->get_parameter("replan_angle_max").as_double();
	node->declare_parameter<double>("car_length",car_length_);
	car_length_ = node->get_parameter("car_length").as_double();
	node->declare_parameter<double>("car_width",car_width_);
	car_width_ = node->get_parameter("car_width").as_double();
	node->declare_parameter<bool>("navigation_estop_flag",navigation_estop_flag_);
	navigation_estop_flag_ = node->get_parameter("navigation_estop_flag").as_bool();
	node->declare_parameter<double>("estop_point_x",estop_point_x_);
	estop_point_x_ = node->get_parameter("estop_point_x").as_double();
	node->declare_parameter<double>("estop_point_y",estop_point_y_);
	estop_point_y_ = node->get_parameter("estop_point_y").as_double();
	node->declare_parameter<double>("dis_tolerance",dis_tolerance_);
	dis_tolerance_ = node->get_parameter("dis_tolerance").as_double();
	node->declare_parameter<double>("max_speed",max_speed_);
	max_speed_ = node->get_parameter("max_speed").as_double();
	node->declare_parameter<double>("max_speed_tolerance",max_speed_tolerance_);
	max_speed_tolerance_ = node->get_parameter("max_speed_tolerance").as_double();
	node->declare_parameter<double>("max_turn_speed",max_turn_speed_);
	max_turn_speed_ = node->get_parameter("max_turn_speed").as_double();
	node->declare_parameter<double>("two_line_length",two_line_length_);
	two_line_length_ = node->get_parameter("two_line_length").as_double();
	node->declare_parameter<double>("track_new_line_speed",track_new_line_speed_);
	track_new_line_speed_ = node->get_parameter("track_new_line_speed").as_double();
	node->declare_parameter<double>("max_omg",max_omg_);
	max_omg_ = node->get_parameter("max_omg").as_double();
	node->declare_parameter<double>("start_speed_limit",controlParamInfo_.start_speed_limit);
	controlParamInfo_.start_speed_limit = node->get_parameter("start_speed_limit").as_double();
	node->declare_parameter<double>("start_acc_limit",controlParamInfo_.start_acc_limit);
	controlParamInfo_.start_acc_limit = node->get_parameter("start_acc_limit").as_double();
	node->declare_parameter<double>("acc_limit",controlParamInfo_.acc_limit);
	controlParamInfo_.acc_limit = node->get_parameter("acc_limit").as_double();
	node->declare_parameter<double>("dec_limit",controlParamInfo_.dec_limit);
	controlParamInfo_.dec_limit = node->get_parameter("dec_limit").as_double();
	node->declare_parameter<double>("obs_brake_dec",controlParamInfo_.obs_brake_dec);
	controlParamInfo_.obs_brake_dec = node->get_parameter("obs_brake_dec").as_double();
	node->declare_parameter<double>("obs_estop_dec",controlParamInfo_.obs_estop_dec);
	controlParamInfo_.obs_estop_dec = node->get_parameter("obs_estop_dec").as_double();
	node->declare_parameter<double>("estop_speed_level",controlParamInfo_.estop_speed_level);
	controlParamInfo_.estop_speed_level = node->get_parameter("estop_speed_level").as_double();
	node->declare_parameter<double>("current2start_dis",current2start_dis_);
	current2start_dis_ = node->get_parameter("current2start_dis").as_double();
	node->declare_parameter<double>("current2start_dis_max",current2start_dis_max_);
	current2start_dis_max_ = node->get_parameter("current2start_dis_max").as_double();
	node->declare_parameter<double>("actuator_delaytime",controlParamInfo_.actuator_delaytime);
	controlParamInfo_.actuator_delaytime = node->get_parameter("actuator_delaytime").as_double();
	node->declare_parameter<double>("actuator_delaytime_1",controlParamInfo_.actuator_delaytime_1);
	controlParamInfo_.actuator_delaytime_1 = node->get_parameter("actuator_delaytime_1").as_double();
	node->declare_parameter<double>("actuator_delaytime_2",controlParamInfo_.actuator_delaytime_2);
	controlParamInfo_.actuator_delaytime_2 = node->get_parameter("actuator_delaytime_2").as_double();
	node->declare_parameter<double>("line_offsety",controlParamInfo_.line_offsety);
	controlParamInfo_.line_offsety = node->get_parameter("line_offsety").as_double();
	node->declare_parameter<double>("line_offsety_max",controlParamInfo_.line_offsety_max);
	controlParamInfo_.line_offsety_max = node->get_parameter("line_offsety_max").as_double();
	node->declare_parameter<double>("curve_offsety",controlParamInfo_.curve_offsety);
	controlParamInfo_.curve_offsety = node->get_parameter("curve_offsety").as_double();
	node->declare_parameter<double>("curve_offsety_max",controlParamInfo_.curve_offsety_max);
	controlParamInfo_.curve_offsety_max = node->get_parameter("curve_offsety_max").as_double();
	node->declare_parameter<double>("travel_dis_max",controlParamInfo_.travel_dis_max);
	controlParamInfo_.travel_dis_max = node->get_parameter("travel_dis_max").as_double();
	node->declare_parameter<double>("stop_dis_tolerance_straight",controlParamInfo_.stop_dis_tolerance_straight);
	controlParamInfo_.stop_dis_tolerance_straight = node->get_parameter("stop_dis_tolerance_straight").as_double();
	node->declare_parameter<double>("stop_dis_tolerance_curve",controlParamInfo_.stop_dis_tolerance_curve);
	controlParamInfo_.stop_dis_tolerance_curve = node->get_parameter("stop_dis_tolerance_curve").as_double();
	node->declare_parameter<double>("curve_insert_dis",controlParamInfo_.curve_insert_dis);
	controlParamInfo_.curve_insert_dis = node->get_parameter("curve_insert_dis").as_double();
	node->declare_parameter<double>("offsety_tolerance",controlParamInfo_.offsety_tolerance);
	controlParamInfo_.offsety_tolerance = node->get_parameter("offsety_tolerance").as_double();
	node->declare_parameter<double>("change_control_param_dis",controlParamInfo_.change_control_param_dis);
	controlParamInfo_.change_control_param_dis = node->get_parameter("change_control_param_dis").as_double();
	node->declare_parameter<double>("offsety_control_level_1",controlParamInfo_.offsety_control_level_1);
	controlParamInfo_.offsety_control_level_1 = node->get_parameter("offsety_control_level_1").as_double();
	node->declare_parameter<double>("offsety_control_level_2",controlParamInfo_.offsety_control_level_2);
	controlParamInfo_.offsety_control_level_2 = node->get_parameter("offsety_control_level_2").as_double();
	node->declare_parameter<double>("offsetangle_tolerance",controlParamInfo_.offsetangle_tolerance);
	controlParamInfo_.offsetangle_tolerance = node->get_parameter("offsetangle_tolerance").as_double();
	node->declare_parameter<double>("offsetangle_tolerance1",controlParamInfo_.offsetangle_tolerance1);
	controlParamInfo_.offsetangle_tolerance1 = node->get_parameter("offsetangle_tolerance1").as_double();
	node->declare_parameter<double>("offsetangle_control_level_1",controlParamInfo_.offsetangle_control_level_1);
	controlParamInfo_.offsetangle_control_level_1 = node->get_parameter("offsetangle_control_level_1").as_double();
	node->declare_parameter<double>("offsetangle_control_level_2",controlParamInfo_.offsetangle_control_level_2);
	controlParamInfo_.offsetangle_control_level_2 = node->get_parameter("offsetangle_control_level_2").as_double();
	node->declare_parameter<double>("creep_distance",controlParamInfo_.creep_distance);
	controlParamInfo_.creep_distance = node->get_parameter("creep_distance").as_double();
	node->declare_parameter<double>("creep_speed",controlParamInfo_.creep_speed);
	controlParamInfo_.creep_speed = node->get_parameter("creep_speed").as_double();
	node->declare_parameter<double>("arrive_target_tolerance",controlParamInfo_.arrive_target_tolerance);
	controlParamInfo_.arrive_target_tolerance = node->get_parameter("arrive_target_tolerance").as_double();
	node->declare_parameter<std::string>("path_info_", controlParamInfo_.m_path_info);
	controlParamInfo_.m_path_info = node->get_parameter("path_info_").as_string();
	node->declare_parameter<bool>("u_turn_enable", u_turn_enable_);
	u_turn_enable_ = node->get_parameter("u_turn_enable").as_bool();
	node->declare_parameter<double>("pid_tolerance",controlParamInfo_.pid_tolerance);
	controlParamInfo_.pid_tolerance = node->get_parameter("pid_tolerance").as_double();
	node->declare_parameter<double>("integrator_saturation_offsety",controlParamInfo_.integrator_saturation_offsety);
	controlParamInfo_.integrator_saturation_offsety = node->get_parameter("integrator_saturation_offsety").as_double();
	node->declare_parameter<double>("integrator_saturation_heading",controlParamInfo_.integrator_saturation_heading);
	controlParamInfo_.integrator_saturation_heading = node->get_parameter("integrator_saturation_heading").as_double();
	node->declare_parameter<int>("pid_diff_mode_offsety",controlParamInfo_.pid_diff_mode_offsety);
	controlParamInfo_.pid_diff_mode_offsety = node->get_parameter("pid_diff_mode_offsety").as_int();
	node->declare_parameter<int>("pid_diff_mode_heading",controlParamInfo_.pid_diff_mode_heading);
	controlParamInfo_.pid_diff_mode_heading = node->get_parameter("pid_diff_mode_heading").as_int();
	node->declare_parameter<bool>("endpoint_rotate_enable", controlParamInfo_.endpoint_rotate_enable);
	controlParamInfo_.endpoint_rotate_enable = node->get_parameter("endpoint_rotate_enable").as_bool();
	#endif


	RCLCPP_INFO(node->get_logger(),"Paramaters loading finished.");
	// 初始化
	Init();
	setOffsetyPID();
	setHeadingPID();

	location_sub_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped >("/mrpt_pose", 1, std::bind(&ControlTruck::locationCallback, this, std::placeholders::_1));
	agvstatus_sub_1_ = node->create_subscription<geometry_msgs::msg::Twist >("/real_speed", 1, std::bind(&ControlTruck::motorInfoCallback, this, std::placeholders::_1));
	//agvstatus_sub_ = node->create_subscription<geometry_msgs::msg::Twist >("/odom", 1, std::bind(&ControlTruck::agvStateInfoCallback, this, std::placeholders::_1));
	path_index_sub_ = node->create_subscription<std_msgs::msg::Int32 >("/path_index",10,std::bind(&ControlTruck::ChangePathIndexCallBack, this, std::placeholders::_1));
	angle_encoder_sub_ = node->create_subscription<std_msgs::msg::Float32 >("/AngleEncoder", 1, std::bind(&ControlTruck::AngleEncoderCallback, this, std::placeholders::_1));
	// path_info_pub_   = node->create_publisher< nav_msgs::msg::Path >("/control_track_point", 10);//发布pathpoint pose


	nav_cmd_sub_ = node->create_subscription<ubt_interface::msg::NavReq>("/nav_req",10, std::bind(&ControlTruck::partPathInfoCallback, this, std::placeholders::_1));
	timer_            = node->create_wall_timer(std::chrono::milliseconds(int((1.0/cycle_frequency_)*1000)),std::bind(&ControlTruck::task_management, this));
	control_type_pub_ = node->create_publisher< std_msgs::msg::Int32>("/control_type", 10);//发布命令
	Control_debug_pub_ = node->create_publisher< std_msgs::msg::Float32MultiArray >("/Control_debug", 10);//发布命令
	path_index_pub_= node->create_publisher< std_msgs::msg::Int32 >("/path_index", 10);//

	arrivetarget_pub2_= node->create_publisher<ubt_interface::msg::NavResp>("/nav_resp_slam", 10);//发布指令执行完成，到达目标点的标志
	arrivetarget_pub_= node->create_publisher< std_msgs::msg::String >("/arrived_target", 10);//发布指令执行完成，到达目标点的标志
	nav_resp_pub_= node->create_publisher<ubt_interface::msg::NavResp>("/nav_resp", 10);//发布指令执行完成，到达目标点的标志
	nav_fail_pub_ = node->create_publisher< std_msgs::msg::Int32 >("/nav_fail", 10);//发布nav偏航flag

	path_index_slam_sub_ = node->create_subscription<std_msgs::msg::Int32 >("/path_index_slam",10,std::bind(&ControlTruck::SlamPathIndexCallBack, this, std::placeholders::_1));
	map_confidence_sub_ = node->create_subscription<std_msgs::msg::Float32 >("/map_confidence", 10, std::bind(&ControlTruck::MapConfidenceCallback, this, std::placeholders::_1));
	// nav_resp_slam_sub_ = node->create_subscription<ubt_interface::msg::NavResp>("/nav_resp_slam", 10, &ControlTruck::_HandleNavResp, this);
	control_cmd_pub_ = node->create_publisher< geometry_msgs::msg::Twist >("/cmd_vel", 10);//发布命令（v,w）
	control_cmd_debug_pub_ = node->create_publisher< geometry_msgs::msg::Twist >("/cmd_vel_debug", 10);//发布命令（v,w），仅调试显示，非实际生效
	path_index_slam_pub_= node->create_publisher< std_msgs::msg::Int32 >("/path_index_slam", 10);//
	nav_req_slam_pub_ = node->create_publisher<ubt_interface::msg::NavReq>("/nav_req_slam", 10); //主要用于结束slam任务

	direct_cmd_vel_pub_ = node->create_publisher< std_msgs::msg::Float32MultiArray >("/direct_cmd_vel", 10);//发布电机控制命令
	// rfid_sub_ = node->create_subscription<ubt_interface::msg::Rfid >("/rfid_state",10,std::bind(&ControlTruck::RfidCallBack, this, std::placeholders::_1));
	lift_sub_ = node->create_subscription<std_msgs::msg::UInt32 >("/traction_latch_state",10,std::bind(&ControlTruck::LiftCallBack, this, std::placeholders::_1));

	// dynamic_reconfigure::Server<ubt_trajectory_track::pidConfig> config_server;
	// dynamic_reconfigure::Server<ubt_trajectory_track::pidConfig>::CallbackType f;
	// f = boost::bind(std::bind(&ControlTruck::reconfigureCallback, this, std::placeholders::_1));
	// config_server.setCallback(f);

}
// void ControlTruck::_HandleNavResp(const ubt_interface::msg::NavResp &navResp)
void ControlTruck::_HandleNavResp(void)
{
	slam_finish_flag_ = 1;
	ubt_interface::msg::NavResp respon;
	respon.status=3;
	nav_resp_pub_->publish(respon);

}

void ControlTruck::MapConfidenceCallback(const std_msgs::msg::Float32::SharedPtr confidence)
{
	map_confidence_     = confidence->data;
	UpdatePositionState();
}

void ControlTruck::LiftCallBack(const std_msgs::msg::UInt32::SharedPtr msg)
{
	if(msg->data == 1){
		lift_state_ = LiftState::LIFT ;
	}else if(msg->data == 2){
		lift_state_ = LiftState::DOWN;

	}
}

void ControlTruck::CheckTakFinish(void)
{
	if(new_task_flag_ == 1 && slam_finish_flag_ == 1 ){
		ubt_interface::msg::NavResp respon;
		respon.status=3;
		nav_resp_pub_->publish(respon);
	}
}

void ControlTruck::NewTask(void)
{
	new_task_flag_ = 1;
	slam_finish_flag_ = 0;
}

void ControlTruck::UpdatePositionState(void)
{
	std_msgs::msg::Int32 data;
	data.data = 1;
	control_type_pub_->publish(data);
	CheckTakFinish();
}

void ControlTruck::SlamPathIndexCallBack(const std_msgs::msg::Int32::SharedPtr msg)
{
	if(position_state_ == 2) return ;
	std_msgs::msg::Int32 index;
	index.data = msg->data;
	path_index_pub_->publish(index);

}

bool ControlTruck::OnlineCheck(void)
{
	int first_path_index = 0;
	PathInfoType first_path   = RouteInfo_[first_path_index];
	Pose_type start_point     = first_path.PathPointInfo[0];
	Pose_type end_point       = first_path.PathPointInfo[1];
	double first_path_length  = sqrt(pow(end_point.x - start_point.x,2) + pow(end_point.y - start_point.y,2));

	if(first_path_length <= 0.05 && RouteInfo_.size() > 1)
	{
		first_path_index = first_path_index + 1;
		first_path   = RouteInfo_[first_path_index];
		start_point     = first_path.PathPointInfo[0];
		end_point       = first_path.PathPointInfo[1];
		double first_path_length  = sqrt(pow(end_point.x - start_point.x,2) + pow(end_point.y - start_point.y,2));
	}
	Pose_type projection_;
    if (first_path.PathType == "LINE")
    {
		// if(sqrt((start_point.x - end_point.x) * (start_point.x - end_point.x) + (start_point.y - end_point.y)*(start_point.y - end_point.y)) < 0.02)
		// {
		// 	return false;
		// }
        projection_  = getTruckProjectPoint2Line(start_point,end_point,agv_body_point_);
		RCLCPP_ERROR(node->get_logger(),"st.x:%f,st.y:%f,e.x:%f,e.y:%f,a.x:%f,a.y:%f,p.x:%f,p.y:%f",start_point.x,start_point.y,end_point.x,end_point.y,agv_body_point_.x,agv_body_point_.y,projection_.x,projection_.y);

		double offset_y      = sqrt((agv_body_point_.x - projection_.x) * (agv_body_point_.x - projection_.x) + (agv_body_point_.y - projection_.y)*(agv_body_point_.y - projection_.y));
		printf(" Control OnlineCheck offset_y:%f \r\n",offset_y);

		if(fabs(offset_y) < 1.5 && ((projection_.x - start_point.x ) * (projection_.x - end_point.x ) <= 0 && (projection_.y - start_point.y ) * (projection_.y - end_point.y ) <= 0))
		{
			double straightAngle = new_line_tracking_.getLaneHeading(start_point,end_point);
			double offset_heading_      = straightAngle - agv_body_point_.theta;
			offset_heading_      = wrapToPI(offset_heading_);
			printf(" Control OnlineCheck offset_heading_:%f \r\n",offset_heading_);
			if(lift_state_ == LiftState::DOWN){
				// 牵引货物的时候不允许旋转过多
				if(fabs(offset_heading_) < controlParamInfo_.offsetangle_tolerance * 1.5 ){
					ExecutingStep_    = ExecutingStep::TrackPath;
					PublishPathIndexMsg(path_index_);//add by byf 20210309
					PublishRotateFinishMsg(path_index_);//add 20210422
					printf(" Control OnlineCheck change ExecutingStep_:TrackPath \r\n");
					return true;
				}
				else if(fabs(offset_heading_) < PI/4){
					ExecutingStep_    = ExecutingStep::TurnToStartPoint;
					printf(" Control OnlineCheck change ExecutingStep_:TurnToStartPoint \r\n");
					return true;
				}else{
					RCLCPP_ERROR(node->get_logger(),"offset_heading_ to large");
					AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_TRACK_FAILED;
					std_msgs::msg::Int32 index;
					index.data = 2; // 2 : 牽引銷降下不允許大角度轉動
					nav_fail_pub_->publish(index);
					return true;
				}
			}else if(lift_state_ == LiftState::LIFT){
				if(fabs(offset_heading_) < controlParamInfo_.offsetangle_tolerance){
					ExecutingStep_    = ExecutingStep::TrackPath;
					PublishPathIndexMsg(path_index_);//add by byf 20210309
					PublishRotateFinishMsg(path_index_);//add 20210422
					printf(" Control OnlineCheck change ExecutingStep_:TrackPath \r\n");
					return true;
				}
				else if(fabs(offset_heading_) < PI/2){
					ExecutingStep_    = ExecutingStep::TurnToStartPoint;
					printf(" Control OnlineCheck change ExecutingStep_:TurnToStartPoint \r\n");
					return true;
				}
			}
			

		}
    }
	else if (first_path.PathType == "BESSEL")
	{
		Pose_type control_point       = first_path.PathPointInfo[2];
		Pose_type agv_point;
		agv_point.x = agv_body_point_.x;
		agv_point.y = agv_body_point_.y;
		agv_point.heading = agv_body_point_.theta;
		double straightAngle = new_line_tracking_.getLaneHeading(agv_point,start_point);
		double offset_start_heading_      = straightAngle - agv_body_point_.theta;
		offset_start_heading_      = wrapToPI(offset_start_heading_);
		straightAngle = new_line_tracking_.getLaneHeading(agv_point,end_point);
		double offset_end_heading_      = straightAngle - agv_body_point_.theta;
		offset_end_heading_      = wrapToPI(offset_end_heading_);
		// printf(" Control OnlineCheck offset_start_heading_:%f offset_end_heading_:%f,\r\n",offset_start_heading_,offset_end_heading_);
		if(fabs(offset_start_heading_) > PI/2 && fabs(offset_end_heading_) < PI/2)   //起点在后面，终点在前面
		{
			ExecutingStep_    = ExecutingStep::TrackPath;
			PublishPathIndexMsg(path_index_);//add by byf 20210309
			PublishRotateFinishMsg(path_index_);//add 20210422
			printf(" Control OnlineCheck change ExecutingStep_:TrackPath \r\n");
			return true;
		}

	}else
	{
		return false;
	}
	if(lift_state_ == LiftState::DOWN){
		double straightAngle = new_line_tracking_.getLaneHeading(start_point,end_point);
		double offset_heading_      = straightAngle - agv_body_point_.theta;
		offset_heading_      = wrapToPI(offset_heading_);
		if(fabs(offset_heading_) > PI/4){
			RCLCPP_ERROR(node->get_logger(),"offset_heading_ to large");
			std_msgs::msg::Int32 index;
			index.data = 1; // 1 : 偏航
			nav_fail_pub_->publish(index);
			AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_TRACK_FAILED;
		}
	}

	return false;

}




void ControlTruck::ChangePathIndexCallBack(const std_msgs::msg::Int32::SharedPtr msg)
{
	int new_index = msg->data;
	if(new_index != path_index_ && AgvExecuteCmdState_step_ != VmsCmdExecuteState::WAIT && new_index >= 0)
	{
		RCLCPP_INFO(node->get_logger(),"recrive new_index:  %d",new_index);
		path_index_ = new_index;
		ExecutingStep_            = ExecutingStep::TrackPath;
		single_path_track_step_   = SinglePathTrackStep::Track;
		vel_cmd_ = real_send_speed_;
		stop_step_                = 0;
		centerpoint_init_.x       = center_point_.x;
		centerpoint_init_.y       = center_point_.y;
		centerpoint_init_.heading = center_point_.theta;
		dec_new_                  = 0.0;
		travel_distance_          = 0.0;
	}

}

void ControlTruck::partPathInfoCallback(const ubt_interface::msg::NavReq::SharedPtr nav_cmd_msg)
{

    int	 ubt_cmd = nav_cmd_msg->cmd;
	//将状态机的状态值进行保存
	RCLCPP_INFO(node->get_logger(),"##############get new order##############    %d",ubt_cmd);
	if (ubt_cmd==4)
	{

		state_value_ = 4;
		// ubt_state_ = "UBT_TRAFFIC";
		std::cout<<"            traffic           "<<std::endl;
		// ubt_state_two="TRAFFIC_";
		RCLCPP_INFO(node->get_logger(),"ubt_state_two###############= %s",ubt_state_two.c_str());
		Pose_type tra_pose;
		tra_pose.x=nav_cmd_msg->traffic_point.x;
		tra_pose.y=nav_cmd_msg->traffic_point.y;
		tra_pose.heading=nav_cmd_msg->traffic_point.t;
		tra_pose.id=nav_cmd_msg->traffic_point.id;
		tra_pose.curvature=nav_cmd_msg->traffic_point.avoid_dis;
		// RouteInfo_.clear();
		TrafficRun(nav_cmd_msg->traffic_point_index, tra_pose);
		// TrafficRun(tra_pose);

	}
	else if (ubt_cmd==1)
	{
		NewTask();
		state_value_ = 1;
		ubt_state_ = "UBT_RUN";
		ubt_state_two="RUN_";
		RCLCPP_INFO(node->get_logger(),"ubt_state_two###############= %s",ubt_state_two.c_str());
		std::cout<<"            run            "<<std::endl;
		spdlog::info("ubt_cmd = 1,start task.");
	}
	else if (ubt_cmd==2)
	{
		state_value_ = 2;
		ubt_state_ ="UBT_PAUSE";
	}
	else if (ubt_cmd==0)
	{
		state_value_ = 0;
		ubt_state_ = "UBT_STOP";
	}
	else if (ubt_cmd==3)
	{
		state_value_ = 3;
		ubt_state_ = "UBT_CONTINUE";
	}
	else if (ubt_cmd==5)
	{
		state_value_ = 5;
		ubt_state_ = "UBT_SLOWDOWN";
	}
	else if (ubt_cmd==6)
	{
		state_value_ = 6;
		// ubt_state_ = "UBT_TRAFFIC_CONTINUE";
		// ubt_state_two="RUN_";
		RCLCPP_INFO(node->get_logger(),"ubt_state_two###############= %s",ubt_state_two.c_str());
		RCLCPP_INFO(node->get_logger(),"Control--Task : CONTINUE  RUN      mission  before ,after traffic");
		// RouteInfo_.clear();
		ContinueTrafficRun();
	}
	else
	{
		state_value_ = -1;
	}



    if(ubt_cmd==1)
	{
	//任务下发

		try
		{
				ubt_interface::msg::NavInfo pathinfo_req = nav_cmd_msg->path_info;
				//导航信息：路线序列lines
				const std::vector<ubt_interface::msg::Line> &pathlist_ =  pathinfo_req.path;
				// 解析前做一下接口路径清理
				VarResetZero();
				RouteInfo_.clear();
				navigation_start_ = true;

				for(auto it = pathlist_.begin(); it != pathlist_.end(); ++it)
				{
					//定义算法路径接口临时保存
					PathInfoType pathInfo;
					pathInfo.PathId = 0;
					pathInfo.PathType = it->type;
					// 将it->points写入pathInfo.PathPointInfo
					GetPathPointInfo(pathInfo.PathPointInfo, it->points);
			

					pathInfo.v_max =it->limit.x;
					pathInfo.w_max =it->limit.w;
					RouteInfo_.emplace_back(pathInfo);
				}

				// 测试：打印RouteInfo
				for(auto it = RouteInfo_.begin(); it != RouteInfo_.end(); ++it)
				{
						auto itp_start= it->PathPointInfo.begin();
						auto itp_end=  itp_start+1;
						RCLCPP_INFO(node->get_logger(),"Time:%.2f\n [ubt_trajectory_track]: topic /nav_req  receive  cmd: %d , RouteInfo is \n pathID: %d\n PathType:%s\n start point:(%lf,%lf,%lf)\n end point: (%lf,%lf,%lf)\n",rclcpp::Clock().now().seconds(), 1,it->PathId,
						it->PathType.c_str(),
						itp_start->x,
						itp_start->y,
						itp_start->heading,
						itp_end->x,
						itp_end->y,
						itp_end->heading);
				}
		}
		catch (...)
		{
			/* code for Catch */
			RCLCPP_INFO(node->get_logger(),"RUN CMD faild");
		}


		RouteInfo_save.clear();
		for(auto it = RouteInfo_.begin(); it != RouteInfo_.end(); ++it)
		{
			//定义算法路径接口临时保存
			PathInfoType pathInfo;
			pathInfo.PathId = 0;
			pathInfo.PathType = it->PathType;
			// 将it->points写入pathInfo.PathPointInfo
			GetPathPointInfo_(pathInfo.PathPointInfo, it->PathPointInfo);
			pathInfo.v_max =it->v_max;
			pathInfo.w_max =it->w_max;
			RouteInfo_save.emplace_back(pathInfo);
		}


		for(auto it = RouteInfo_save.begin(); it != RouteInfo_save.end(); ++it)
				{
						auto itp_start= it->PathPointInfo.begin();
						auto itp_end=  itp_start+1;
						RCLCPP_INFO(node->get_logger(),"save  Time:%.2f\n [ubt_trajectory_track]: topic /nav_req  receive  cmd: %d , RouteInfo is \n pathID: %d\n PathType:%s\n start point:(%lf,%lf,%lf)\n end point: (%lf,%lf,%lf)\n",rclcpp::Clock().now().seconds(), 1,it->PathId,
						it->PathType.c_str(),
						itp_start->x,
						itp_start->y,
						itp_start->heading,
						itp_end->x,
						itp_end->y,
						itp_end->heading);
				}


		}

	Task_complete_flag_ = "NULL";

	RCLCPP_INFO(node->get_logger(),"ubt_state_ partPathInfoCallback ##################=%s",ubt_state_.c_str());
}


void ControlTruck::TrafficRun(int trafficPointIndex, Pose_type traffic_pos )
// void ControlTruck::TrafficRun(Pose_type traffic_pos )

{
	traffic_info_.flag = 1;
	traffic_info_.pointIndex = trafficPointIndex;
	traffic_info_.point.x=traffic_pos.x;
	traffic_info_.point.y=traffic_pos.y;
	traffic_info_.point.heading=traffic_pos.heading;
	traffic_info_.point.id=traffic_pos.id;
	RCLCPP_INFO(node->get_logger()," TrafficRun ");
}

void ControlTruck::ContinueTrafficRun()
{
	ClearTrafficInfo();
	RCLCPP_INFO(node->get_logger()," ContinueTrafficRun ");

}

void ControlTruck::ClearTrafficInfo(void)
{
	traffic_info_.flag = 0;
	traffic_info_.point.x=0;
	traffic_info_.point.y=0;
	traffic_info_.point.heading=0;
	traffic_info_.point.curvature=0;
	traffic_info_.point.id=0;
	traffic_info_.point.direction_index=0;
	traffic_info_.point.max_v=0;
	traffic_info_.point.rfid.clear();


}

void ControlTruck::SlowDown()
{
	try{

		stop_obs_flag_=2;
		//stop_step_=1;
		ubt_state_="UBT_RUN";
	}
	catch (...){
		/* code for Catch */
		RCLCPP_INFO(node->get_logger(),"CONTINUE CMD faild");
	}

}

void ControlTruck::GetPathPointInfo(std::vector<Pose_type> &pathPointInfo, const std::vector<ubt_interface::msg::Point> &pointInfo)
{
	for(auto it = pointInfo.begin(); it != pointInfo.end(); ++it)
	{
		Pose_type poseType;
		poseType.rfid = it->rfid;
		poseType.x = it->x;
		poseType.y = it->y;
		poseType.heading = it->t;
		poseType.id = it->id;
		pathPointInfo.emplace_back(poseType);
	}
}


void ControlTruck::GetPathPointInfo_(std::vector<Pose_type> &pathPointInfo, const std::vector<Pose_type> &pointInfo)
{
	for(auto it = pointInfo.begin(); it != pointInfo.end(); ++it)
	{
		Pose_type poseType;
		poseType.x = it->x;
		poseType.y = it->y;
		poseType.heading = it->heading;
		poseType.id = it->id;
		pathPointInfo.emplace_back(poseType);
	}
}

void ControlTruck::SpdlogInit(void)
{
	std::string home_name = getenv("HOME");
	std::string node_name = "control_node_truck";
    // auto console_logger = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
    auto file_logger = std::make_shared<spdlog::sinks::rotating_file_sink_mt>(home_name + "/.ros/log/spdlog/" + node_name + "/"  + node_name + ".log",1024 * 1024 * 10, 3);
    auto logger = std::make_shared<spdlog::logger>("task_logger", spdlog::sinks_init_list({file_logger}));
    spdlog::set_default_logger(logger);
    spdlog::set_level(spdlog::level::trace);
    spdlog::flush_on(spdlog::level::err);
    spdlog::flush_every(std::chrono::seconds(1));
	// spdlog::set_pattern("[%H:%M:%S %z] [%^---%L---%$] [thread %t] %v");
	spdlog::info(">>>>>>>>>>>>>>>>>>>>>>>start<<<<<<<<<<<<<<<<<<<<<<<.");
    spdlog::info("Spdlog init success.");
}

void ControlTruck::Init()
{
	RouteInfo_.clear();
	New_route_.clear();
	Ready_location_           = 0;
	Estop_flag_               = false;
	rotate_stop_flag_         = false;
	Task_cancel_flag_         = 0;
	stop_step_                = 0;
	path_index_               = 0;
	new_path_index_           = 0;
	stop_save_data_count_     = 0;
	stop_obs_flag_            = 0;
	turn_direction_           = 0;
	state_value_              = 0;//add 20210616增加状态机的值保存

	center_point_.x           = 0.0;
	center_point_.y           = 0.0;
	center_point_.theta       = 0.0;
	pre_center_point_.x       = 0.0;
	pre_center_point_.y       = 0.0;
	pre_center_point_.theta   = 0.0;
	centerpoint_init_.x       = 0.0;
	centerpoint_init_.y       = 0.0;
	centerpoint_init_.heading = 0.0;
	vehicle_speed_            = 0.0;
	vehicle_angular_          = 0.0;

	vehicle_motor_speed_      = 0.0;
	vehicle_motor_angular_    = 0.0;
	vehicle_odom_speed_       = 0.0;
	vehicle_odom_angular_     = 0.0;
	odom_pose_x_              = 0.0;
	odom_pose_y_              = 0.0;
	odom_pose_theta_          = 0.0;
	vel_cmd_                  = 0.0;
	w_cmd_                    = 0.0;
	pre_w_cmd_                = 0.0;

	target_speed_        	  = 0.0;
	init_angle_               = 0.0;
	cycletime_                = 1.0/cycle_frequency_;
	task_time_                = 0.0;
	track_time_               = 0.0;
	save_time_                = 0.0;
	dec_new_                  = 0.0;
	travel_distance_          = 0.0;//add 20210315

	read_yaml_flag_           = false;
	navigation_start_         = false;//add 20210603
	CMD_finish_flag_          = false;
	stop_save_data_flag_      = false;
	Task_complete_flag_       = "NULL";
	ubt_state_                = "UBT_WAIT";
	ubt_state_pre_            = "UBT_WAIT";
	ubt_state_two			  = "WAIT_";
	lift_state_               = LiftState::DOWN;
	AgvExecuteCmdState_step_  = VmsCmdExecuteState::WAIT;
	ExecutingStep_            = ExecutingStep::TurnToNewLine;
	single_path_track_step_   = SinglePathTrackStep::Track;
	trajectorytracking_.Clear();
	new_line_tracking_.Clear();
	TraceDataCount            = 0;
	control_data_vec_.clear();
	CreatTraceFolder();
	ClearTrafficInfo();
}

void ControlTruck::setOffsetyPID()
{
	pid_conf_offsety_.kp  = controlParamInfo_.adaptive_kp_line;
	pid_conf_offsety_.ki  = controlParamInfo_.adaptive_ki_line;
	pid_conf_offsety_.kd  = controlParamInfo_.adaptive_kd_line;
	pid_control_offsety_.init(pid_conf_offsety_);
	pid_control_offsety_.setIntegralLimit(controlParamInfo_.integrator_saturation_offsety,-controlParamInfo_.integrator_saturation_offsety);
	pid_control_offsety_.setDiffMode(controlParamInfo_.pid_diff_mode_offsety);
}

void ControlTruck::setHeadingPID()
{
	pid_conf_heading_.kp  = controlParamInfo_.adaptive_kp_heading_line;
	pid_conf_heading_.ki  = controlParamInfo_.adaptive_ki_heading_line;
	pid_conf_heading_.kd  = controlParamInfo_.adaptive_kd_heading_line;
	pid_control_heading_.init(pid_conf_heading_);
	pid_control_heading_.setIntegralLimit(controlParamInfo_.integrator_saturation_heading,-controlParamInfo_.integrator_saturation_heading);
	pid_control_heading_.setDiffMode(controlParamInfo_.pid_diff_mode_heading);
}

void ControlTruck::locationCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr location_msg)
{
	// 函数功能：处理收到的位置信息
	geometry_msgs::msg::Pose2D car_point;
	car_point.x     = location_msg->pose.pose.position.x;
   	car_point.y     = location_msg->pose.pose.position.y;
   	tf2::Quaternion quat(0,0,location_msg->pose.pose.orientation.z,location_msg->pose.pose.orientation.w);
	tf2::Matrix3x3 m(quat);
	double roll, pitch, yaw;
	m.getRPY(roll, pitch, yaw);

   	car_point.theta = wrapToPI(yaw) ;
	location_mutex_.lock();
	agv_body_point_    = car_point;
	car_point.theta =  wrapToPI(car_point.theta + diff_angle_);
	center_point_      = car_point;
	location_mutex_.unlock();
	Ready_location_    = 1;
	// printf("Control--RobotLocation: ---car_point-(%f,%f,%f)---\r\n",car_point.x,car_point.y,car_point.theta);
}

void ControlTruck::AngleEncoderCallback(const std_msgs::msg::Float32::SharedPtr angle)
{
	double new_angle = angle->data;
	diff_angle_ = angle2Radian(-new_angle);
	location_mutex_.lock();
	center_point_.theta      = wrapToPI(agv_body_point_.theta + diff_angle_);
	// printf("Control--RobotLocation: ---car_point-(%f,%f,%f)---\r\n",center_point_.x,center_point_.y,center_point_.theta);
	location_mutex_.unlock();
}

void ControlTruck::motorInfoCallback(const geometry_msgs::msg::Twist::SharedPtr agvstatus_msg)
{
	// 函数功能：处理收到的AGV状态信息
	vehicle_motor_speed_   = agvstatus_msg->linear.x;
	vehicle_motor_angular_ = agvstatus_msg->angular.z;
	vehicle_speed_         = agvstatus_msg->linear.x;
	vehicle_angular_       = agvstatus_msg->angular.z;

}


void ControlTruck::agvStateInfoCallback(const nav_msgs::msg::Odometry::SharedPtr agvstatus_msg)
{
	// 函数功能：处理收到的AGV状态信息
	vehicle_odom_speed_   = agvstatus_msg->twist.twist.linear.x;
	vehicle_odom_angular_ = agvstatus_msg->twist.twist.angular.z;
	vehicle_speed_   = agvstatus_msg->twist.twist.linear.x;
	vehicle_angular_ = agvstatus_msg->twist.twist.angular.z;
	odom_pose_x_ = agvstatus_msg->pose.pose.position.x;
	odom_pose_y_ = agvstatus_msg->pose.pose.position.y;
	tf2::Quaternion quat(0,0,agvstatus_msg->pose.pose.orientation.z,agvstatus_msg->pose.pose.orientation.w);
	tf2::Matrix3x3 m(quat);
	double roll, pitch, yaw;
	m.getRPY(roll, pitch, yaw);
   	odom_pose_theta_ = yaw;
}

void ControlTruck::SetSpeedOmgMsg(double v,double w)
{
	//发布车辆中心点v,w控制命令
	// geometry_msgs::msg::Twist control_cmd;
	// if(w != 0 && v != 0 &&fabs(w - pre_w_cmd_ ) < 0.05) {
	// 	// RCLCPP_WARN(node->get_logger(),"------------------------:%f,%f",w,pre_w_cmd_);
	// 	w = pre_w_cmd_;
	
	// }
	w = (w>max_omg_ ? max_omg_ : (w<-max_omg_ ? -max_omg_ : w));

	control_cmd_.linear.x = v;
	control_cmd_.linear.y = 0.0;
	control_cmd_.linear.z = 0.0;

	control_cmd_.angular.x = 0.0;
	control_cmd_.angular.y = 0.0;
	control_cmd_.angular.z = w;
	// RCLCPP_WARN(node->get_logger(),"Control--SetSpeedOmgMsg: publish v w cmd,v = %f,w = %f",v,w);
	control_cmd_debug_pub_->publish(control_cmd_);
	real_send_speed_ = v;
	control_cmd_pub_->publish(control_cmd_);
	
	// pre_w_cmd_ = w;

}

void ControlTruck::PublishArrivedTargetMsg(std::string arrived_flag)
{
	std_msgs::msg::String arrived_target;
	arrived_target.data = arrived_flag;
	arrivetarget_pub_->publish(arrived_target);

}

void ControlTruck::PublishPathIndexMsg(int path_index)
{
	char buf[64];
	std_msgs::msg::String arrived_target;
	sprintf(buf,"TASK_PROGRESS %d",path_index);
	arrived_target.data = std::string(buf);
	arrivetarget_pub_->publish(arrived_target);
	std_msgs::msg::Int32 index;
	index.data = path_index;
	path_index_slam_pub_->publish(index);
}

void ControlTruck::PublishRotateFinishMsg(int path_index)
{
	char buf[64];
	std_msgs::msg::String arrived_target;
	sprintf(buf,"TASK_ROTATE %d",path_index);
	arrived_target.data = std::string(buf);
	arrivetarget_pub_->publish(arrived_target);
}

void ControlTruck::PublishBrakeMsg()
{
	w_cmd_   = 0.0;
	vel_cmd_ = vel_cmd_ - controlParamInfo_.obs_brake_dec * (1.0/cycle_frequency_);
	vel_cmd_ = vel_cmd_ <= 0.05 ? 0.0 : vel_cmd_;
	SetSpeedOmgMsg(vel_cmd_,w_cmd_);
}

void ControlTruck::PublishEstopMsg()
{
	// 下发急停命令
	// 将原有的策略直接下发速度为0，改成分段减速停车，临界速度可调 modify 20210629
	w_cmd_ = 0.0;
	if (vel_cmd_ > controlParamInfo_.estop_speed_level + EPSILON)
	{

		vel_cmd_ = vel_cmd_ - controlParamInfo_.obs_estop_dec * (1.0/cycle_frequency_);
		vel_cmd_ = vel_cmd_ <= controlParamInfo_.estop_speed_level ? controlParamInfo_.estop_speed_level : vel_cmd_;
	}
	else if(vel_cmd_==0)
	{
			ubt_state_ = "UBT_WAIT";
	}
	else
	{

		vel_cmd_ = vel_cmd_ - controlParamInfo_.obs_brake_dec * (1.0/cycle_frequency_);
		vel_cmd_ = vel_cmd_ <= 0.05 ? 0.0 : vel_cmd_;
	}
	SetSpeedOmgMsg(vel_cmd_,w_cmd_);
}

void ControlTruck::CreatTraceFolder()
{
	if(-1 == access("TraceData",0))
	{
		if(0 != mkdir("TraceData",S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH))
		{
			RCLCPP_WARN(node->get_logger(),"Control--CreatTraceFolder:Cannot Creat TraceLog Folder.");
		}
	}
	if(-1 == access("TraceData/Control",0))
	{
		if(0 != mkdir("TraceData/Control",S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH))
		{
			RCLCPP_WARN(node->get_logger(),"Control--CreatTraceFolder:Cannot Creat ControlData Folder.");
		}
	}
}
 
void ControlTruck::TraceData()
{
	if(Task_complete_flag_ == "TASK_FINISH" || Task_complete_flag_ == "TASK_ABORT" || Task_complete_flag_ == "NULL")
	{
		stop_save_data_count_++;
		if(TraceDataCount == 0)
		{
			stop_save_data_count_ = 0;
			return;
		}
		if(stop_save_data_count_ > 20)
		{
			stop_save_data_flag_  = true;
			stop_save_data_count_ = 0;
		}
	}
	else if(Task_complete_flag_ == "TASK_WAIT")
	{
		return;
	}
	else
	{
		stop_save_data_count_ = 0;
		stop_save_data_flag_  = false;
	}

	TraceDataCount ++;

	// 需要保存的控制变量
	Pose_type pathstartpoint   = trajectorytracking_.PathStartPoint();
	Pose_type pathendpoint     = trajectorytracking_.PathEndPoint();
	Pose_type pathcontrolpoint = trajectorytracking_.PathControlPoint();
	double    w_offsety                    = trajectorytracking_.getW_offsety();
	double    w_offsetheading     = trajectorytracking_.getW_offsetheading();
	std::string control_buf = ",";
 	control_buf = control_buf +
							   std::to_string(center_point_.x) + "," +
							   std::to_string(center_point_.y) + "," +
 							   std::to_string(center_point_.theta) + "," +
							   std::to_string(vel_cmd_) + "," +
							   std::to_string(w_cmd_) + "," +
							   std::to_string(vehicle_speed_) + "," +
							   std::to_string(vehicle_angular_) + "," +
							   std::to_string(pathstartpoint.x) + "," +
							   std::to_string(pathstartpoint.y) + "," +
							   std::to_string(pathstartpoint.heading) + "," +
							   std::to_string(pathendpoint.x) + "," +
							   std::to_string(pathendpoint.y) + "," +
							   std::to_string(pathendpoint.heading) + "," +
							   std::to_string(pathcontrolpoint.x) + "," +
							   std::to_string(pathcontrolpoint.y) + "," +
							   std::to_string(pathcontrolpoint.heading) + "," +
							   std::to_string(trajectorytracking_.getOffsetY()) + "," +
							   std::to_string(trajectorytracking_.getOffsetX()) + "," +
							   std::to_string(trajectorytracking_.getOffsetHeading()) + "," +
							   std::to_string(trajectorytracking_.getOffsetSpeed()) + "," +
							   std::to_string(trajectorytracking_.TrajectoryTracking_complete()) + "," +
							   std::to_string(Ready_location_) + "," +
							   std::to_string(trajectorytracking_.getTargetDis()) + "," +
							   std::to_string(trajectorytracking_.getDecNew()) + "," +
							   std::to_string(task_time_) + "," +
							   std::to_string(save_time_) + "," +
							   std::to_string(track_time_) + "," +
							   std::to_string(vehicle_motor_speed_) + "," +
							   std::to_string(vehicle_motor_angular_) + "," +
							   std::to_string(vehicle_odom_speed_) + "," +
							   std::to_string(vehicle_odom_angular_) + "," +
							   std::to_string(odom_pose_x_) + "," +
							   std::to_string(odom_pose_y_) + "," +
							   std::to_string(odom_pose_theta_) + "," +
							   std::to_string(cos_theta_) + "," +
							   std::to_string(projection_point_.x) + "," +
							   std::to_string(projection_point_.y) + "," +
							   std::to_string(travel_distance_) + "," +
							   std::to_string(w_offsety) + "," +
							   std::to_string(w_offsetheading) + "," +
							   std::to_string(state_value_) + "," +
							   std::to_string(stop_obs_flag_) + "," +
							   std::to_string(navigation_start_) + "," +
							   std::to_string(CMD_finish_flag_) + "," +
							   std::to_string(Estop_flag_);

	control_data_vec_.push_back(control_buf);
	if((TraceDataCount == MAX_TRACE_NUM) || (Estop_flag_ == true) || (stop_save_data_flag_ == true))
	{
		//RCLCPP_INFO(node->get_logger(),"Control--TraceData: Start Save trace file .");
		CreatTraceFolder();
		time_t t = time(NULL);
		tm *local = localtime(&t);
		std::string current_time = std::to_string(local->tm_year+1900) +
									(local->tm_mon+1 < 10?("0"+std::to_string(local->tm_mon+1)):std::to_string(local->tm_mon+1)) +
									(local->tm_mday < 10?("0"+std::to_string(local->tm_mday)):std::to_string(local->tm_mday)) + "_" +
									(local->tm_hour < 10?("0"+std::to_string(local->tm_hour)):std::to_string(local->tm_hour)) + "_" +
									(local->tm_min < 10?("0"+std::to_string(local->tm_min)):std::to_string(local->tm_min)) + "_" +
									(local->tm_sec < 10?("0"+std::to_string(local->tm_sec)):std::to_string(local->tm_sec));
		std::string control_data = "TraceData/Control/ControlData_" + current_time + ".txt";
		std::ofstream control_data_file_(control_data);
		if(control_data_file_.is_open())
		{
			for(uint32_t i = 0; i < control_data_vec_.size();i++ )
			{
				control_data_file_ << control_data_vec_[i] << std::endl;
			}
			control_data_file_.close();
			//RCLCPP_INFO(node->get_logger(),"Control--TraceData: Save Control trace file OK .");
		}
		else
		{
			RCLCPP_WARN(node->get_logger(),"Control--TraceData: Create Control trace file failed.");
		}
		TraceDataCount       = 0;
		Estop_flag_          = false;
		stop_save_data_flag_ = false;
		control_data_vec_.clear();
	}
	DeleteRedundantTraceFiles();
}


void ControlTruck::DebugOutput()
{

	std_msgs::msg::Float32MultiArray control_debug;

	control_debug.data.push_back(vel_cmd_*1000.0);
	control_debug.data.push_back(w_cmd_*1000.0);
	control_debug.data.push_back(trajectorytracking_.getOffsetY()*1000.0);
	control_debug.data.push_back(trajectorytracking_.getOffsetHeading()*1000.0);
	control_debug.data.push_back(trajectorytracking_.getW_offsety()*1000.0);
	control_debug.data.push_back(trajectorytracking_.getW_offsetheading()*1000.0);
	Control_debug_pub_->publish(control_debug);
 
}

void ControlTruck::DeleteRedundantTraceFiles()
{
	// 遍历文件夹中的所有文件,删除文件夹中多余的文件 20201012
	DIR *dir=NULL;
	struct dirent* pDir=NULL;
	std::string Fliname_Oldest;
	// 删除决策文件中多余的文件
	int FilesNum = 0;
  	dir = opendir("TraceData/Control");
	if(dir == NULL)
	{
		RCLCPP_WARN(node->get_logger(),"Control--DeleteRedundantTraceFiles: Can't open Control dir");
		return;
	}
	while(true)
	{
		pDir = readdir(dir);
		if((pDir == NULL))
		{
			break;
		}
		if (pDir->d_type == DT_REG)
		{
			// 找出文件名最小的一个文件，即生成时间最早的一个文件
			FilesNum ++;
			if(FilesNum == 1)
			{
				Fliname_Oldest = pDir ->d_name;
			}
			else
			{
				if(Fliname_Oldest > pDir ->d_name)
				{
					Fliname_Oldest = pDir ->d_name;
				}
			}
		}
	}
	closedir(dir);
	if(FilesNum > MAX_FILE_NUM)
	{
		std::string deletfn = "TraceData/Control/" + Fliname_Oldest;
		if(-1 == remove(deletfn.c_str()))
		{
			RCLCPP_WARN(node->get_logger(),"Control--DeleteRedundantTraceFiles: delete Control trace file failed.");
			return;
		}
	}
}

void ControlTruck::VarResetZero()
{
	RouteInfo_.clear();
	path_index_              = 0;
	new_path_index_          = 0;
	stop_obs_flag_           = 0;
	stop_step_               = 0;  //20210201
	dec_new_                 = 0.0;//20210201
	travel_distance_         = 0.0;//add 20210524
	vel_cmd_                 = 0.0;
	w_cmd_                   = 0.0;
	AgvExecuteCmdState_step_ = VmsCmdExecuteState::WAIT;
	ExecutingStep_           = ExecutingStep::TurnToNewLine;
	single_path_track_step_  = SinglePathTrackStep::Track;
	ClearTrafficInfo();
}

void ControlTruck::TaskWait()
{
	Task_complete_flag_  = "TASK_WAIT";
	int route_count = RouteInfo_.size();
	if (route_count > 0)
	{
		RCLCPP_WARN(node->get_logger(),"Control--Task: receive new path, start to execute");
		CMD_finish_flag_          = false;
		centerpoint_init_.x       = center_point_.x;
		centerpoint_init_.y       = center_point_.y;
		centerpoint_init_.heading = center_point_.theta;
		AgvExecuteCmdState_step_  = VmsCmdExecuteState::EXECUTING;
		Task_complete_flag_       = "TASK_EXECUTING";
	}
	else
	{
		RCLCPP_WARN(node->get_logger(),"Control--Task: waiting new path!");
	}
	PublishArrivedTargetMsg(Task_complete_flag_);
}

void ControlTruck::TaskExecuting()
{
	// RCLCPP_WARN(node->get_logger(),"Control-task_management: path_index_ = %d,RouteInfo_.size = %d,ExecutingStep_ = %d,AgvExecuteCmdState_step_ = %d,max_omg_ = %f",
	        //  path_index_,int(RouteInfo_.size()),int(ExecutingStep_),int(AgvExecuteCmdState_step_),max_omg_);
	// RCLCPP_WARN(node->get_logger(),RouteInfo_[0].PathType);
	switch(ExecutingStep_)
	{
		case ExecutingStep::TurnToNewLine:
			{
				RCLCPP_ERROR(node->get_logger(),"Control--Task : TurnToNewLine");

				if(!OnlineCheck())
				{
					TurnToNewLine();
				}
				// RCLCPP_INFO(node->get_logger(),"Control--Task : TurnToNewLine");
			}
				break;
		case ExecutingStep::TrackNewLine:
			{
				// OnlineCheck();
				RCLCPP_ERROR(node->get_logger(),"Control--Task : TrackNewLine");

				TrackNewLine();
			}
				break;
		case ExecutingStep::TurnToStartPoint:
			{
				// if(!OnlineCheck())
				RCLCPP_ERROR(node->get_logger(),"Control--Task : TurnToStartPoint");

				{
					TurnToStartPoint();
					RCLCPP_INFO(node->get_logger(),"Control--Task : TurnToStartPoint");

				}

			}
				break;
		case ExecutingStep::TrackPath:
			{
				RCLCPP_ERROR(node->get_logger(),"Control--Task : TrackPath");
				TrackPath();
			}
			break;
		default:
			break;
	}

}


void ControlTruck::TaskFinish()
{
	RouteInfo_.clear();
	stop_step_               = 0;
	real_send_speed_		 = 0;
	path_index_              = 0;
	Task_complete_flag_      = "TASK_FINISH";
	AgvExecuteCmdState_step_ = VmsCmdExecuteState::WAIT;
	ExecutingStep_           = ExecutingStep::TurnToNewLine;
	single_path_track_step_  = SinglePathTrackStep::Track;
	RCLCPP_INFO(node->get_logger(),"Control--Task  5: all path complete,waiting new path!");
	PublishArrivedTargetMsg(Task_complete_flag_);
	if(ubt_state_two=="RUN_")
	{
		path_index_              = 0;
		ubt_interface::msg::NavResp respon;
		respon.status=3;
		RCLCPP_INFO(node->get_logger(),"Control--Task  5: send back state");
		arrivetarget_pub2_->publish(respon);
		_HandleNavResp();
	}
	else
	{
		path_index_++;

	}

	ubt_state_two="WAIT_";
	RCLCPP_INFO(node->get_logger(),"ubt_state_two###############= %s",ubt_state_two.c_str());


}

void ControlTruck::TurnToNewLine()
{

	PathInfoType first_path   = RouteInfo_[0];
	Pose_type start_point     = first_path.PathPointInfo[0];
	Pose_type end_point       = first_path.PathPointInfo[1];

	double init_and_start_dis = sqrt(pow(centerpoint_init_.x - start_point.x,2) + pow(centerpoint_init_.y - start_point.y,2));
	double first_path_length  = sqrt(pow(end_point.x - start_point.x,2) + pow(end_point.y - start_point.y,2));
	double diff_angle = 0;

	if(0)
	{
		double target_line_angle      = new_line_tracking_.getLaneHeading(centerpoint_init_,start_point);
		double current_agv_heading    = agv_body_point_.theta;
		diff_angle = wrapToPI(current_agv_heading - target_line_angle);
		if(fabs(diff_angle)>PI/2 && init_and_start_dis > current2start_dis_)
		{
			AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_EXCEED_3M;
			RCLCPP_WARN(node->get_logger(),"Control-task_management: current point and first path's start point angle diff > PI/2.");
			return;
		}

	}
	

	// RCLCPP_WARN(node->get_logger(),"Control-task_management: init_and_start_dis = %f,first_path_length = %f",init_and_start_dis,first_path_length);
	if(init_and_start_dis > current2start_dis_ && init_and_start_dis <= current2start_dis_max_)
	{
		//旋转一个角度，与new_line的航向角相同
		new_path_.PathPointInfo.clear();
		new_path_.PathId     = 0;
		new_path_.PathType   = "LINE";
		new_path_.PathPointInfo.emplace_back(centerpoint_init_);
		new_path_.PathPointInfo.emplace_back(start_point);
		new_path_.v_max      = track_new_line_speed_;
		max_speed_           = new_path_.v_max;
		target_speed_        = 0.0;
 
		init_angle_                   = centerpoint_init_.heading;
		double current_point_heading  = center_point_.theta;
		double target_line_angle      = new_line_tracking_.getLaneHeading(centerpoint_init_,start_point);
		w_cmd_                        = new_line_tracking_.OmgCommandPlan_new(init_angle_,current_point_heading,target_line_angle,max_omg_,w_cmd_,stop_obs_flag_);
		w_cmd_                        = (w_cmd_>max_omg_ ? max_omg_ : (w_cmd_<-max_omg_ ? -max_omg_ : w_cmd_));
		// RCLCPP_WARN(node->get_logger(),"Control-task_management: init_angle_ = %f,target_line_angle = %f,w_cmd_ = %f",init_angle_,target_line_angle,w_cmd_);
		// RCLCPP_WARN(node->get_logger(),"1 target_line_angle - current_point_heading = %f",fabs(wrapToPI(target_line_angle - current_point_heading)));

		if(fabs(wrapToPI(target_line_angle - current_point_heading)) <= controlParamInfo_.offsetangle_tolerance	)
		{
			rotate_stop_flag_ = true;
		}
		if (rotate_stop_flag_ == true)
		{
			w_cmd_ = 0.0;
			if(fabs(vehicle_angular_) < 0.01)
			{
				rotate_stop_flag_ = false;
				ExecutingStep_    = ExecutingStep::TrackNewLine;
				pid_control_offsety_.reSet();
				pid_control_heading_.reSet();
				int path_index    = -1;//add by byf 20210309
				PublishPathIndexMsg(path_index);
				PublishRotateFinishMsg(path_index);//add 20210422
				RCLCPP_WARN(node->get_logger(),"Control-task_management: new ExecutingStep_:TrackNewLine");
			}
		}
		vel_cmd_ = 0.0;
		SetSpeedOmgMsg(vel_cmd_,w_cmd_);
	}
	else if(init_and_start_dis <= current2start_dis_)
	{
		if(first_path_length<current2start_dis_)
		{
			AgvExecuteCmdState_step_ = VmsCmdExecuteState::FINISH;
			RCLCPP_WARN(node->get_logger(),"Control-task_management: current point and first path's start point and end point are same");
			return;
		}
		else
		{
		init_angle_                  = centerpoint_init_.heading;
		double current_point_heading = center_point_.theta;
		double target_line_angle     = 0.0;
		if(first_path_length <= EPSILON)		
		{
			target_line_angle = start_point.heading;
		}
		else
		{
			target_line_angle = new_line_tracking_.getLaneHeading(start_point,end_point);
			if( first_path.PathType == "BESSEL")
			{
				Pose_type control_point  = first_path.PathPointInfo[2];
				target_line_angle        = new_line_tracking_.getLaneHeading(start_point,control_point);
			}
		}
		w_cmd_ = new_line_tracking_.OmgCommandPlan_new(init_angle_,current_point_heading,target_line_angle,max_omg_,w_cmd_,stop_obs_flag_);
		w_cmd_ = (w_cmd_>max_omg_ ? max_omg_ : (w_cmd_<-max_omg_ ? -max_omg_ : w_cmd_));
		// RCLCPP_WARN(node->get_logger(),"Control-task_management: current W_CMD:%f",w_cmd_);
		// RCLCPP_WARN(node->get_logger(),"2 target_line_angle - current_point_heading = %f",fabs(wrapToPI(target_line_angle - current_point_heading)));
		// RCLCPP_WARN(node->get_logger(),"centerpoint_init_.heading: %f,center_point_.theta:%f",centerpoint_init_.heading,center_point_.theta);

		if(fabs(wrapToPI(target_line_angle - current_point_heading)) <= controlParamInfo_.offsetangle_tolerance)
		{
			rotate_stop_flag_ = true;
			//RCLCPP_WARN(node->get_logger(),"Control-task_management: rotate_stop_flag_");
		}
		if (rotate_stop_flag_ == true)
		{
			w_cmd_ = 0.0;
			RCLCPP_WARN(node->get_logger(),"Control-task_management: vehicle_angular_%f",fabs(vehicle_angular_));
			if(fabs(vehicle_angular_) < 0.01)
			{
				rotate_stop_flag_ = false;
				ExecutingStep_    = ExecutingStep::TrackPath;
			    PublishPathIndexMsg(path_index_);//add by byf 20210309
				PublishRotateFinishMsg(path_index_);//add 20210422
			}
		}
		vel_cmd_ = 0.0;
		SetSpeedOmgMsg(vel_cmd_,w_cmd_);

		}

	}
	else
	{
		AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_EXCEED_3M;
		RCLCPP_WARN(node->get_logger(),"Control-task_management: current point and first path's start point distance is too large > 3m.");
		return;
	}
}

void ControlTruck::TrackNewLine()
{
	if(fabs(real_send_speed_ - vel_cmd_) > 0.1)
	{
		vel_cmd_ = real_send_speed_;
	}
	track_start_        = clock();
	trajectorytracking_  = TrajectoryTrackingTruck(new_path_, center_point_, max_speed_,max_omg_,target_speed_,dec_new_,vehicle_speed_,vel_cmd_,w_cmd_,cycletime_,stop_step_,stop_obs_flag_,turn_direction_,travel_distance_,pid_control_offsety_,pid_control_heading_,controlParamInfo_);

	track_end_          = clock();
	track_time_         = (double)(track_end_ - track_start_) / CLOCKS_PER_SEC * 1000;
	vel_cmd_            = trajectorytracking_.getVelCmd();
	w_cmd_              = trajectorytracking_.getWCmd();
	stop_step_          = trajectorytracking_.getStopStep();
	dec_new_            = trajectorytracking_.getDecNew();
	travel_distance_    = trajectorytracking_.getTravelDistance();
	if (trajectorytracking_.TrajectoryTracking_complete() == 1)
	{
		vel_cmd_                  = 0.0;
		ExecutingStep_            = ExecutingStep::TurnToStartPoint;
		stop_step_                = 0;
		centerpoint_init_.x       = center_point_.x;
		centerpoint_init_.y       = center_point_.y;
		centerpoint_init_.heading = center_point_.theta;
		dec_new_ = 0.0;
		travel_distance_ = 0.0;
		printf(" Control  change ExecutingStep_:TurnToStartPoint \r\n");
	}
	//add 20210315 当路径偏差超过最大偏差时，如果在2米距离之内还没有纠偏回来，则急停保护停车
	/*if (travel_distance_ >= controlParamInfo_.travel_dis_max)
	{
		AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_TRACK_FAILED;
	}*/
	// RCLCPP_WARN(node->get_logger(),"travel_distance_:%f",travel_distance_);
	SetSpeedOmgMsg(vel_cmd_,w_cmd_);
}

void ControlTruck::TurnToStartPoint()
{
	vel_cmd_                  = 0.0;
	PathInfoType first_path       = RouteInfo_[0];
	Pose_type start_point         = first_path.PathPointInfo[0];
	Pose_type end_point           = first_path.PathPointInfo[1];


	init_angle_                   = centerpoint_init_.heading;
	double current_point_heading  = center_point_.theta;
	double target_line_angle      = new_line_tracking_.getLaneHeading(start_point,end_point);
	double path_length            = sqrt(pow(end_point.x - start_point.x,2) + pow(end_point.y - start_point.y,2));
	if(path_length <= 0.02 )
	{
		if(controlParamInfo_.endpoint_rotate_enable)
		{
			target_line_angle  = start_point.heading;
		}
		else
		{
			rotate_stop_flag_ = true;
			RCLCPP_INFO(node->get_logger(),"Control::TurnToStartPoint :dis of start_point and start_point < 0.02");
		}

	}
	if( first_path.PathType == "BESSEL")
	{
		Pose_type control_point  = first_path.PathPointInfo[2];
		target_line_angle        = new_line_tracking_.getLaneHeading(start_point,control_point);
	}
	w_cmd_  = new_line_tracking_.OmgCommandPlan_new(init_angle_,current_point_heading,target_line_angle,max_omg_,w_cmd_,stop_obs_flag_);
	w_cmd_  = (w_cmd_>max_omg_ ? max_omg_ : (w_cmd_<-max_omg_ ? -max_omg_ : w_cmd_));
	if(lift_state_ == LiftState::DOWN){
		double offset_heading_      = target_line_angle - current_point_heading;
		offset_heading_      = wrapToPI(offset_heading_);
		if(fabs(offset_heading_) > PI/4){
			RCLCPP_ERROR(node->get_logger(),"offset_heading_ to large");
			std_msgs::msg::Int32 index;
			index.data = 2;
			nav_fail_pub_->publish(index);
			AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_TRACK_FAILED;
			return;
		}
	}
	// RCLCPP_WARN(node->get_logger(),"3 target_line_angle - current_point_heading = %f",fabs(wrapToPI(target_line_angle - current_point_heading)));
	if(fabs(wrapToPI(target_line_angle - current_point_heading)) <= controlParamInfo_.offsetangle_tolerance)
	{
		rotate_stop_flag_ = true;
	}
	if (rotate_stop_flag_ == true)
	{
		w_cmd_ = 0.0;
		if(fabs(vehicle_angular_) < 0.01)
		{
			rotate_stop_flag_ = false;
			ExecutingStep_    = ExecutingStep::TrackPath;
			pid_control_offsety_.reSet();
			pid_control_heading_.reSet();
			PublishPathIndexMsg(path_index_);//add by byf 20210309
			PublishRotateFinishMsg(path_index_);//add 20210422
		}
	}
	
	SetSpeedOmgMsg(vel_cmd_,w_cmd_);
}

void ControlTruck::TrackPath()
{
	if (path_index_ < int(RouteInfo_.size()))
	{
		int          next_path_index    = 0;//add 20210317
		bool         no_valid_next_path = false;//add 20210323
		PathInfoType path               = RouteInfo_[path_index_];
		std::string  current_path_type  = path.PathType;
		max_speed_                      = std::min(path.v_max,max_speed_tolerance_);
		if(current_path_type == "BESSEL")
		{
			double dis_start_control = sqrt(pow(RouteInfo_[path_index_].PathPointInfo[0].x - RouteInfo_[path_index_].PathPointInfo[2].x,2) +
											pow(RouteInfo_[path_index_].PathPointInfo[0].y - RouteInfo_[path_index_].PathPointInfo[2].y,2));
			double dis_end_control = sqrt(pow(RouteInfo_[path_index_].PathPointInfo[1].x - RouteInfo_[path_index_].PathPointInfo[2].x,2) +
										  pow(RouteInfo_[path_index_].PathPointInfo[1].y - RouteInfo_[path_index_].PathPointInfo[2].y,2));
            double two_line_length = dis_start_control + dis_end_control;
			if (two_line_length <= 1.5*two_line_length_ && two_line_length > two_line_length_)
			{
				max_turn_speed_ = 0.3;
			}
			else if (two_line_length <= two_line_length_ && two_line_length > 0.5*two_line_length_)
			{
				max_turn_speed_ = 0.15;
			}
			else if (two_line_length <= 0.5*two_line_length_)
			{
				max_turn_speed_ = 0.1;
			}
			else
			{
				max_turn_speed_ = 0.5;
			}
			max_speed_   = std::min(path.v_max,max_turn_speed_);
			//RCLCPP_WARN(node->get_logger(),"Control-task_management: max_speed_ = %f,max_turn_speed_ = %f",max_speed_,max_turn_speed_);
		}
		//calculate target speed
		Pose_type startpoint_1 = RouteInfo_[path_index_].PathPointInfo[0];
		Pose_type endpoint_1   = RouteInfo_[path_index_].PathPointInfo[1];
		double path_length     = sqrt(pow(endpoint_1.x - startpoint_1.x,2) + pow(endpoint_1.y - startpoint_1.y,2));

		if(path_index_ == int(RouteInfo_.size())-1)
		{
			//add 20210317
			if(path_length <= EPSILON)
			{
				AgvExecuteCmdState_step_ = VmsCmdExecuteState::FINISH;
				RCLCPP_WARN(node->get_logger(),"Control-task_management: line has two same points.");
				return;
			}
			target_speed_ = 0.0;
		}
		else
		{
			//add by byf 20210317
			//从下一条路径开始搜索，找到距离当前路径最近的有效路径，作为下一条路径
			//对异常路径进行过滤（起点和终点相同的路径）
			if(path_length <= EPSILON)
			{
				path_index_ = path_index_+1;
				PublishPathIndexMsg(path_index_);//add by byf 20210317
				PublishRotateFinishMsg(path_index_);//add 20210422
				RCLCPP_WARN(node->get_logger(),"Control-task_management: current line has two same points.");
				return;
			}
			Pose_type startpoint_2      = RouteInfo_[path_index_+1].PathPointInfo[0];
			Pose_type endpoint_2        = RouteInfo_[path_index_+1].PathPointInfo[1];
			std::string next_path_type  = RouteInfo_[path_index_+1].PathType;
			double path_length_1        = sqrt(pow(endpoint_2.x - startpoint_2.x,2) + pow(endpoint_2.y - startpoint_2.y,2));
			double current_line_angle   = new_line_tracking_.getLaneHeading(startpoint_1,endpoint_1);
			double next_line_angle      = new_line_tracking_.getLaneHeading(startpoint_2,endpoint_2);
			double two_line_delta_theta = wrapToPI(current_line_angle - next_line_angle);
			for (size_t i = path_index_ + 1; i <= int(RouteInfo_.size())-1; i++)
			{
				startpoint_2   = RouteInfo_[i].PathPointInfo[0];
				endpoint_2     = RouteInfo_[i].PathPointInfo[1];
				next_path_type = RouteInfo_[i].PathType;
				path_length_1  = sqrt(pow(endpoint_2.x - startpoint_2.x,2) + pow(endpoint_2.y - startpoint_2.y,2));
				double current_end_next_start_dis = sqrt(pow(endpoint_1.x - startpoint_2.x,2) + pow(endpoint_1.y - startpoint_2.y,2));//add 20210322

				if (path_length_1 > EPSILON && current_end_next_start_dis < EPSILON)//add 20210320
				{
					next_path_index    = i;
					no_valid_next_path = false;
					break;
				}
				if (i == int(RouteInfo_.size())-1)
				{
					if (path_length_1 > EPSILON && current_end_next_start_dis < EPSILON)
					{
						next_path_index    = i;
						no_valid_next_path = false;
					}
					else
					{
						next_path_index    = path_index_;
						no_valid_next_path = true;
					}
				}
			}
			startpoint_2         = RouteInfo_[next_path_index].PathPointInfo[0];
			endpoint_2           = RouteInfo_[next_path_index].PathPointInfo[1];
			current_line_angle   = new_line_tracking_.getLaneHeading(startpoint_1,endpoint_1);
			next_line_angle      = new_line_tracking_.getLaneHeading(startpoint_2,endpoint_2);
			two_line_delta_theta = wrapToPI(current_line_angle - next_line_angle);

			if (no_valid_next_path == true)
			{
				target_speed_ = 0.0;
			}
			else
			{
				if(current_path_type == "LINE")
				{
					if(next_path_type == "LINE")
					{
						//
					}
					else
					{
						Pose_type controlpoint_2 = RouteInfo_[next_path_index].PathPointInfo[2];
						next_line_angle          = new_line_tracking_.getLaneHeading(startpoint_2,controlpoint_2);
					}
				}
				else
				{
					if(next_path_type == "LINE")
					{
						Pose_type controlpoint_1 = RouteInfo_[path_index_].PathPointInfo[2];
						current_line_angle       = new_line_tracking_.getLaneHeading(controlpoint_1,endpoint_1);
						next_line_angle          = new_line_tracking_.getLaneHeading(startpoint_2,endpoint_2);
					}
					else
					{
						Pose_type controlpoint_1 = RouteInfo_[path_index_].PathPointInfo[2];
						Pose_type controlpoint_2 = RouteInfo_[next_path_index].PathPointInfo[2];
						current_line_angle       = new_line_tracking_.getLaneHeading(controlpoint_1,endpoint_1);
						next_line_angle          = new_line_tracking_.getLaneHeading(startpoint_2,controlpoint_2);
					}
				}
				two_line_delta_theta   = wrapToPI(current_line_angle - next_line_angle);
				// RCLCPP_WARN(node->get_logger(),"Control--Task : current_line_angle = %f,next_line_angle = %f,two_line_delta_theta = %f,replan_angle_max_ = %f,replan_angle_min_ = %f",
				// 							current_line_angle,next_line_angle,two_line_delta_theta*180.0/PI,replan_angle_max_,replan_angle_min_);
				if (fabs(two_line_delta_theta) <= replan_angle_min_*PI/180.0 + EPSILON)
				{
					double v1 = std::max(sqrt(2.0*controlParamInfo_.acc_limit*path_length),vel_cmd_);
					double v2 = max_speed_;
					if(current_path_type == "BESSEL")
					{
						v2 = max_turn_speed_;
					}
					double v3 = std::min(RouteInfo_[next_path_index].v_max,max_speed_tolerance_);
					if(next_path_type == "BESSEL")
					{
						v3 = std::min(std::min(RouteInfo_[next_path_index].v_max,max_turn_speed_),max_speed_tolerance_);
					}
					target_speed_ = std::min(std::min(v1,v2),v3);
					// RCLCPP_WARN(node->get_logger(),"Control--Task : v1 = %f,v2 = %f,v3 = %f",v1,v2,v3);
				}
				else if (fabs(two_line_delta_theta) <= replan_angle_max_*PI/180.0 + EPSILON)
				{
					double v1 = std::max(sqrt(2.0*controlParamInfo_.acc_limit*path_length),vel_cmd_);
					double v2 = 0.3;
					if(current_path_type == "BESSEL")
					{
						v2 = max_turn_speed_;
					}
					double v3 = std::min(RouteInfo_[next_path_index].v_max,max_speed_tolerance_);
					if(next_path_type == "BESSEL")
					{
						v3 = std::min(std::min(RouteInfo_[next_path_index].v_max,max_turn_speed_),max_speed_tolerance_);
					}
					target_speed_ = std::min(std::min(v1,v2),v3);
					// RCLCPP_WARN(node->get_logger(),"Control--Task : v1 = %f,v2 = %f,v3 = %f",v1,v2,v3);
				}
				else
				{
					target_speed_ = 0.0;
				}
			}
		}
		// RCLCPP_INFO(node->get_logger(),"Control--Task :path_index_ = %d,traffic_info_.flag = %d.",path_index_,traffic_info_.flag);
		if(traffic_info_.flag == 1 && path_index_ >= traffic_info_.pointIndex)     //交管
		{
			if(RouteInfo_[path_index_].PathPointInfo[0].id == traffic_info_.point.id )
			{
				max_speed_ = 0.0;
				target_speed_ = 0.0;
				if(real_send_speed_ < 0.01)
				{
					// stop_obs_flag_ = 1;
				}
			}else if(RouteInfo_[path_index_].PathPointInfo[1].id == traffic_info_.point.id )
			{
				target_speed_ = 0.0;
			}
		}

		// RCLCPP_INFO(node->get_logger(),"Control--Task :path_index_ = %d,current_path_type = %s.",path_index_,current_path_type.c_str());
		// RCLCPP_INFO(node->get_logger(),"Control--Task :max_speed_ = %f,target_speed_ = %f,vel_cmd_ = %f,single_path_track_step_ = %d",max_speed_,target_speed_,vel_cmd_,single_path_track_step_);
		switch (single_path_track_step_)
		{
			case SinglePathTrackStep::Track:
				{
					track_start_        = clock();
					trajectorytracking_ = TrajectoryTrackingTruck(path,center_point_,max_speed_,max_omg_,target_speed_,dec_new_,vehicle_speed_,vel_cmd_,w_cmd_,cycletime_,stop_step_,stop_obs_flag_,turn_direction_,travel_distance_,pid_control_offsety_,pid_control_heading_,controlParamInfo_);
					track_end_          = clock();
					track_time_         = (double)(track_end_ - track_start_) / CLOCKS_PER_SEC * 1000;
					vel_cmd_            = trajectorytracking_.getVelCmd();
					w_cmd_              = trajectorytracking_.getWCmd();
					stop_step_          = trajectorytracking_.getStopStep();
					dec_new_            = trajectorytracking_.getDecNew();
					travel_distance_    = trajectorytracking_.getTravelDistance();
					projection_point_   = trajectorytracking_.getProjectionPoint();
					cos_theta_          = trajectorytracking_.getCosTheta();
					//偏离线过大，急停保护车
					double offset_y     = trajectorytracking_.getOffsetY();
					if(fabs(offset_y) >= 1){
						AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_TRACK_FAILED;
						std_msgs::msg::Int32 index;
						index.data = 1;
						nav_fail_pub_->publish(index);

						return;
					}
					// if(fabs(real_send_speed_ - vel_cmd_) > 0.2)
					// {
					// 	vel_cmd_ = real_send_speed_;
					// }
					SetSpeedOmgMsg(vel_cmd_,w_cmd_);
					if (trajectorytracking_.TrajectoryTracking_complete() == 1)
					{
						if(target_speed_ == 0.0)
						{
							RCLCPP_INFO(node->get_logger(),"track  path  form track to rotate!!");
							single_path_track_step_   = SinglePathTrackStep::Rotate;
							stop_step_                = 0;
							centerpoint_init_.x       = center_point_.x;
							centerpoint_init_.y       = center_point_.y;
							centerpoint_init_.heading = center_point_.theta;
							dec_new_                  = 0.0;
							travel_distance_          = 0.0;
						}
						else
						{
							path_index_ = next_path_index;
							//add 20210323
							if (no_valid_next_path == true)
							{
								path_index_ = int(RouteInfo_.size())-1;
							}
							PublishPathIndexMsg(path_index_);//add by byf 20210309
							PublishRotateFinishMsg(path_index_);//add 20210422
						}
					}
					//add 20210315 当路径偏差超过最大偏差时，如果在1米距离之内还没有纠偏回来，则急停保护停车
					// RCLCPP_WARN(node->get_logger(),"Control--Task 1 :  travel_distance_ = %f,travel_dis_max = %f",travel_distance_,controlParamInfo_.travel_dis_max);
					/*if (travel_distance_ >= controlParamInfo_.travel_dis_max)
					{
						PublishEstopMsg();
						AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_TRACK_FAILED;
						RCLCPP_WARN(node->get_logger(),"Control--Task : path deviation too large---trajectorytracking_.getoffsety() = %f,travel_distance_ = %f ",trajectorytracking_.getOffsetY(),travel_distance_);
						travel_distance_ = 0.0;
					}*/
				}
				break;
			case SinglePathTrackStep::Rotate:
				{
					vel_cmd_ = 0;
					init_angle_                  = centerpoint_init_.heading;
					double current_point_heading = center_point_.theta;
					Pose_type startpoint         = RouteInfo_[path_index_].PathPointInfo[0];
					Pose_type endpoint           = RouteInfo_[path_index_].PathPointInfo[1];
					double target_line_angle     = new_line_tracking_.getLaneHeading(startpoint,endpoint);

					if(path_index_ == int(RouteInfo_.size())-1)
					{
						if(controlParamInfo_.endpoint_rotate_enable)
						{
							target_line_angle = endpoint.heading;
						}
						else
						{
							target_line_angle = center_point_.theta;
							rotate_stop_flag_ = true;
						}

					}
					else
					{
						PathInfoType next_path     = RouteInfo_[next_path_index];//modify 20210318
						std::string next_path_type = next_path.PathType;
						Pose_type start_point      = next_path.PathPointInfo[0];
						Pose_type end_point        = next_path.PathPointInfo[1];
						if(next_path_type == "BESSEL")
						{
							Pose_type control_point_2 = next_path.PathPointInfo[2];
							target_line_angle         = new_line_tracking_.getLaneHeading(start_point,control_point_2);
						}
						else
						{
							target_line_angle = new_line_tracking_.getLaneHeading(start_point,end_point);
							//add 20210323
							if (no_valid_next_path == true)
							{
								target_line_angle = start_point.heading;
							}
						}
					}
					w_cmd_ = new_line_tracking_.OmgCommandPlan_new(init_angle_,current_point_heading,target_line_angle,max_omg_,w_cmd_,stop_obs_flag_);
					w_cmd_ = (w_cmd_>max_omg_ ? max_omg_ : (w_cmd_<-max_omg_ ? -max_omg_ : w_cmd_));
					RCLCPP_WARN(node->get_logger(),"4 target_line_angle - current_point_heading = %f",fabs(wrapToPI(target_line_angle - current_point_heading)));
					
					if(fabs(wrapToPI(target_line_angle - current_point_heading)) <= 0.02 )
					{
						rotate_stop_flag_ = true;
					}
					if (rotate_stop_flag_ == true)
					{
						w_cmd_ = 0.0;
						if(fabs(vehicle_angular_) < 0.01)
						{
							if(path_index_ == int(RouteInfo_.size())-1)
							{
								CMD_finish_flag_ = true;
								path_index_      = path_index_+1;
							}
							else
							{
								path_index_ = next_path_index;
								//add 20210323
								if (no_valid_next_path == true)
								{
									path_index_ = int(RouteInfo_.size())-1;
								}
								PublishPathIndexMsg(path_index_);//add by byf 20210309
								PublishRotateFinishMsg(path_index_);//add 20210422
							}
							rotate_stop_flag_       = false;
							single_path_track_step_ = SinglePathTrackStep::Track;
							pid_control_offsety_.reSet();
							pid_control_heading_.reSet();
						}
					}
					SetSpeedOmgMsg(vel_cmd_,w_cmd_);
				}
				break;
			default:
				break;
		}
	}
	else
	{
		//所有路径执行完成，
		RCLCPP_INFO(node->get_logger(),"Control--Task 4 : all path complete,waiting new path!");
		spdlog::info("Control--Task 4 : all path complete,waiting new path!");
		AgvExecuteCmdState_step_ = VmsCmdExecuteState::FINISH;
		Task_complete_flag_      = "TASK_FINISH";
		PublishArrivedTargetMsg(Task_complete_flag_);
	}
}

void ControlTruck::JudgePoseJump()
{
	double delta_x     = fabs(center_point_.x - pre_center_point_.x);
	double delta_y     = fabs(center_point_.y - pre_center_point_.y);
	double delta_dis   = sqrt(pow(center_point_.x - pre_center_point_.x,2) + pow(center_point_.y - pre_center_point_.y,2));
	double delta_theta = fabs(wrapToPI(center_point_.theta - pre_center_point_.theta));
	double pose_data_jump = vehicle_speed_ * cycletime_ * 10 + position_jump_;
	//当车辆在导航过程中判断位姿跳变，此时的车辆可能是静止状态
	if (navigation_start_ == true)
	{
		if(delta_x > pose_data_jump || delta_y >  pose_data_jump || delta_dis > pose_data_jump  || delta_theta > heading_jump_*PI/180.0)
		{
			RCLCPP_WARN(node->get_logger(),"Control--JudgePoseJump:----AGV position jump too large,position is lost----. car_point(x = %f,y = %f,theta = %f), pre_car_point(x = %f,y = %f,theta = %f),delta_x = %f,delta_y = %f,delta_dis = %f,delta_theta = %f", center_point_.x,center_point_.y,center_point_.theta, pre_center_point_.x,pre_center_point_.y,pre_center_point_.theta,delta_x,delta_y,delta_dis,delta_theta);
			Estop_flag_ = true;
		}
	}
	pre_center_point_ = center_point_;
	//for test
	//////////////////////////////////////////////////
	if (navigation_estop_flag_ == true)
	{
		double delta_x     = fabs(center_point_.x - estop_point_x_);
		double delta_y     = fabs(center_point_.y - estop_point_y_);
		double delta_dis   = sqrt(pow(delta_x,2) + pow(delta_y,2));
		if (delta_dis < dis_tolerance_)
		{
			Estop_flag_ = true;
		}
	}
	//////////////////////////////////////////////////
}

void ControlTruck::task_management()
{
	task_start_   = clock();
//	JudgePoseJump();//对位姿跳变进行检测

	if(Estop_flag_ == true)
	{
		VarResetZero();
		Estop_flag_              = false;
		AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_POSE_JUMP;
	}
	//取消当前任务策略：快速减速停车，此时暂不考虑是否到达目标点
	if(ubt_state_ == "UBT_WAIT" && (ubt_state_pre_ == "UBT_RUN" || ubt_state_pre_ == "UBT_PAUSE" || ubt_state_pre_ == "UBT_PAUSEMANUAL" || ubt_state_pre_ == "UBT_OBSTACLE" || ubt_state_pre_ == "UBT_RECHARGE"))
	{
		Task_cancel_flag_ = true;
	}
	if(Task_cancel_flag_ == true)
	{
		PublishBrakeMsg();
		if(vel_cmd_ == 0.0)
		{
			Task_cancel_flag_ = false;
		}
	}
	if(ubt_state_ == "UBT_WAIT" || ubt_state_ == "UBT_ACTION")
	{
		VarResetZero();
		travel_distance_    = 0.0;//add 20210315
		Task_complete_flag_ = "NULL";
		navigation_start_   = false;
	}
	else if(ubt_state_ == "UBT_NREFAULT" || ubt_state_ == "UBT_STOP")
	{
		RCLCPP_ERROR(node->get_logger(),"Control--Task : UBT_NREFAULT or UBT_STOP --- AGV Estop!");
		PublishEstopMsg();
		stop_step_  = 0;//20210128
		dec_new_    = 0.0;
		VarResetZero();
		Task_complete_flag_ = "TASK_ABORT";//add 20210201
		PublishArrivedTargetMsg(Task_complete_flag_);//add 20210201
	}
	else if(ubt_state_ == "UBT_FORCE_FINISH")
	{
		RCLCPP_ERROR(node->get_logger(),"Control--Task : UBT_FORCE_FINISH --- AGV Estop!");
		PublishEstopMsg();
		stop_step_  = 0;//20210128
		dec_new_    = 0.0;
		VarResetZero();
		Task_complete_flag_ = "TASK_ABORT";//add 20210201
		PublishArrivedTargetMsg(Task_complete_flag_);//add 20210201
		if(vel_cmd_ <= 0.05)
		{
			TaskFinish();
			ubt_state_ = "UBT_WAIT";
		}


	}
	else if(ubt_state_ == "UBT_PAUSE" || ubt_state_ == "UBT_PAUSEMANUAL")
	{

		RCLCPP_WARN(node->get_logger(),"Control--Task : UBT_PAUSE  or UBT_PAUSEMANUAL --- AGV Brake!");
		stop_obs_flag_=1;
		//PublishEstopMsg();
		stop_step_  = 0;//20210128
		dec_new_    = 0.0;
		ubt_state_ = "UBT_RUN";


	}
	else if( ubt_state_ == "UBT_CONTINUE" )
	{
		try
			{

				stop_obs_flag_=0;
				stop_step_=0;
				ubt_state_ = "UBT_RUN";
				RCLCPP_INFO(node->get_logger(),"CONTINUE CMD succeed");
			}
			catch (...)
			{
				/* code for Catch */
				RCLCPP_INFO(node->get_logger(),"CONTINUE CMD faild");
			}
				RCLCPP_INFO(node->get_logger(),"Control--Task : CONTINUE  RUN   before   mission,after pause");



	}

	else if(ubt_state_ == "UBT_TRAFFIC_CONTINUE" )
	{

	}

	else if(ubt_state_ == "UBT_SLOWDOWN")
	{

		if(ubt_state_two!="WAIT_")
		{
			SlowDown();
			RCLCPP_INFO(node->get_logger(),"Control--Task : CONTINUE  RUN   ");
		}


	}
	else if( ( ubt_state_ == "UBT_RUN"&&int(RouteInfo_.size())>0 ) || ubt_state_ == "UBT_RECHARGE"||(ubt_state_ == "UBT_TRAFFIC"&&int(RouteInfo_.size())>0) )
	{
		switch (AgvExecuteCmdState_step_)
		{
			case VmsCmdExecuteState::WAIT:
				{
					TaskWait();
				}
				break;
			case VmsCmdExecuteState::EXECUTING:
				{
					//RCLCPP_INFO(node->get_logger(),"Control--Task : EXECUTING");
					TaskExecuting();
				}
				break;
			case VmsCmdExecuteState::FINISH:
				{
					TaskFinish();
				}
				break;
			case VmsCmdExecuteState::ABORT:
				{
					Task_complete_flag_ = "TASK_ABORT";
					PublishArrivedTargetMsg(Task_complete_flag_);
				}
				break;
			case VmsCmdExecuteState::ABORT_EXCEED_3M:
				{
					Task_complete_flag_ = "TASK_ABORT_EXCEED_3M";
					PublishArrivedTargetMsg(Task_complete_flag_);
				}
				break;
			case VmsCmdExecuteState::ABORT_TRACK_FAILED:
				{
					PublishEstopMsg();
					if(fabs(vehicle_speed_) < SPEED_EPSILON)
					{
						travel_distance_    = 0.0;
						Task_complete_flag_ = "TASK_ABORT_TRACK_FAILED";
						PublishArrivedTargetMsg(Task_complete_flag_);
					}
					RCLCPP_WARN(node->get_logger(),"Control--Task : path deviation too large---");
				}
				break;
			case VmsCmdExecuteState::ABORT_POSE_JUMP:
				{
					PublishEstopMsg();
					if(fabs(vehicle_speed_) < SPEED_EPSILON)
					{
						Task_complete_flag_ = "TASK_ABORT_POSE_JUMP";//add 20210201
						PublishArrivedTargetMsg(Task_complete_flag_);//add 20210201
					}
					RCLCPP_WARN(node->get_logger(),"Control--Task: AGV pose  jump too large---AGV Estop!");
				}
				break;
			default:
				break;
		}
		DebugOutput();
	}
	else
	{
		//
	}
	task_end_      = clock();
	task_time_     = (double)(task_end_ - task_start_) / CLOCKS_PER_SEC * 1000;
	save_start_    = clock();


	// TraceData();
	save_end_      = clock();
	save_time_     = (double)(save_end_ - save_start_) / CLOCKS_PER_SEC * 1000;
	ubt_state_pre_ = ubt_state_;
}

}
